%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sajad Saeedi
% University of New Brunswick
% Copy Right 2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ref: 
% S. Saeedi, L. Paull, M. Trentini, M. Seto, and H. Li, 
% Map Merging Using Hough Peak Matching, 
% Proceedings of the IEEE/RSJ International Conference on 
% Intelligent Robots and Systems (IROS), 
% Vilamoura, Portugal, October 7-12, pp. 4683 - 4688.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Map transformation function
% to keep the size consistemt, only parts of the map has been rotated 
% which is not zero: like rotating a floating thing in a tank of water
%
% some ideas has been used from following link:
% http://stackoverflow.com/questions/1811372/how-to-rotate-image-by-nearest
% -neighbor-interpolation-using-matlab
% the method is to map the pixels in the opposite direction; 
% for each pixel in the rotated image, 
% find the pixel(s) that maps to it in the original image, 
% then the problem becomes much simpler.
% 
%
% INPUT: 
%   map: map to be rotated
%   angle: rotation angle in degree
%   trans: translation vector [x;y]
%
% OUTPUT
%   out: rotated map
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function out = map_transform(map, angle, trans)

im1 = map;
[m,n]= size(im1);        % size of the map
thet = angle*(pi/180);  % rotation in radian
m1 = m*cos(thet)+n*sin(thet);
n1 = m*sin(thet)+n*cos(thet);    
cr = m/2;               % center of image, in row
cc = n/2;               % center of image, in column

x_tr = trans(1);
y_tr = trans(2);

mm = m;%*sqrt(2);
nn = n;%*sqrt(2);
im2 = zeros(mm,nn);
for t=1:mm
   for s=1:nn
          i = uint16((t-mm/2)*cos(thet)+(s-nn/2)*sin(thet)+m/2);
          j = uint16(-(t-mm/2)*sin(thet)+(s-nn/2)*cos(thet)+n/2);
          if i>0 && j>0 && i<=m && j<=n           
              if im1(i,j,:) ~= 0 % this is for floating rotation
                  if (t+y_tr <= mm)&&(s+x_tr <= nn)&& (t+y_tr >0)&&(s+x_tr>0)
                    im2(t+y_tr, s+x_tr, :)=im1(i,j,:);
                  end
              end
          end
   end
end

out = im2;
